///////////////////////////////////////////////////////////////////////////
//                                                                       //
//  Class: CQuaternion                                                   //
//                                                                       //
//  Quaternion to represent rotations.  Each component of the            //
//  quaternion is stored as a doubling point number.                     //
//                                                                       //
///////////////////////////////////////////////////////////////////////////

#ifndef _QUATERNION_
#define _QUATERNION_

#include "mathlib.h"

template<class FT>
class CQuaternion
{

private :

	// Data
	FT			m_s;
	Vector3		m_v;

public :

	// Constructors
	CQuaternion()
	{
		m_s = 1.0f;
	}
	CQuaternion(const FT s, const FT x, const FT y, const FT z)
	{
		m_s = s;
		m_v.set(x, y, z);
	}
	CQuaternion(const CQuaternion &quat)
	{
		Set(quat);
	}
	CQuaternion(const CQuaternion *pQuat)
	{
		Set(pQuat);
	}
	CQuaternion(const Matrix4 &mat)
	{
		SetMatrix(mat);
	}
	CQuaternion(const FT s, const FT *pVector)
	{
		m_s = s;
		m_v.set(pVector);
	}
	CQuaternion(const FT s, const FT &vector)
	{
		m_s = s;
		m_v.set(vector);
	}
	CQuaternion(const Vector3 &vecFrom, const Vector3 &vecTo)
	{
		Vector3& vecHalf = vecTo + vecFrom;
		vecHalf.normalize();
		m_s = vecHalf.dot(vecTo);
		m_v = vecHalf.cross(vecTo);
	}

	virtual ~CQuaternion() { }

	// Debug
	void Trace() const;

	// Data setting
	void Clear()
	{
		Set(1.0f, 0.0f, 0.0f, 0.0f);
	}
	void Set(const CQuaternion *pQuat)
	{
		Set(pQuat->s(), pQuat->v());
	}
	void Set(const CQuaternion &quat)
	{
		Set(quat.s(), quat.v());
	}
	void Set(const FT s, const FT &v)
	{
		m_s = s;
		m_v.set(v);
	}
	void Set(const FT s, const FT *pV)
	{
		m_s = s;
		m_v.set(pV);
	}
	void Set(const FT s, const FT x, const FT y, const FT z)
	{
		m_s = s;
		m_v.set(x, y, z);
	}
	void SetRotation(FT ax, FT ay, FT az, FT radAngle)
	{
		FT halfAngle = radAngle / 2.0;
		m_s = cos(halfAngle);
		m_v.set(ax, ay, az);
		m_v *= sin(halfAngle);
	}
	void SetMatrix(const Matrix4 &mat)
	{
		FT trace = mat.m[0][0] + mat.m[1][1] + mat.m[2][2] + 1.0;
		if (trace > ML_D_HIGH_TOLERANCE) {
			FT s = 0.5 / sqrt(trace);
			m_s = 0.25 / s;
			m_v.set((mat.m[2][1]-mat.m[1][2])*s, (mat.m[0][2]-mat.m[2][0])*s, (mat.m[1][0]-mat.m[0][1])*s);
		} else {
			if (mat.m[0][0]>mat.m[1][1] && mat.m[0][0]>mat.m[2][2]) {
				FT s = 2.0 * sqrt(1.0 + mat.m[0][0] - mat.m[1][1] - mat.m[2][2]);
				m_s = (mat.m[1][2] - mat.m[2][1]) / s;
				m_v.set(0.25*s, (mat.m[0][1]+mat.m[1][0])/s, (mat.m[0][2]+mat.m[2][0])/s);
			} else if (mat.m[1][1] > mat.m[2][2]) {
				FT s = 2.0 * sqrt(1.0 + mat.m[1][1] - mat.m[0][0] - mat.m[2][2]);
				m_s = (mat.m[0][2] - mat.m[2][0]) / s;
				m_v.set((mat.m[0][1]+mat.m[1][0])/s, 0.25*s, (mat.m[1][2]+mat.m[2][1])/s);
			} else {
				FT s = 2.0 * sqrt(1.0 + mat.m[2][2] - mat.m[0][0] - mat.m[1][1]);
				m_s = (mat.m[0][1] - mat.m[1][0]) / s;
				m_v.set((mat.m[0][2]+mat.m[2][0])/s, (mat.m[1][2]+mat.m[2][1])/s, 0.25*s);
			}
		}
	}

	void GetMatrix(FT *mat) const
	{
		FT Nq = LengthSquared();
		FT c  = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
		FT xc = x()*c,	yc = y()*c,		zc = z()*c;
		FT sx = s()*xc,	sy = s()*yc,	sz = s()*zc;
		FT xx = x()*xc,	xy = x()*yc,	xz = x()*zc;
		FT yy = y()*yc,	yz = y()*zc,	zz = z()*zc;
		mat[0]  = FT(1.0 - (yy + zz));
		mat[1]  = FT(xy - sz);
		mat[2]  = FT(xz + sy);
		mat[4]  = FT(xy + sz);
		mat[5]  = FT(1.0 - (xx + zz));
		mat[6]  = FT(yz - sx);
		mat[8]  = FT(xz - sy);
		mat[9]  = FT(yz + sx);
		mat[10] = FT(1.0 - (xx + yy));
		mat[3]  = mat[7] = mat[11] = mat[12] = mat[13] = mat[14] = 0.0f;
		mat[15] = 1.0f;
	}
	Matrix4 GetMatrix() const
	{
		Matrix4 mat;
		FT Nq = LengthSquared();
		FT c  = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
		FT xc = x()*c,	yc = y()*c,		zc = z()*c;
		FT sx = s()*xc,	sy = s()*yc,	sz = s()*zc;
		FT xx = x()*xc,	xy = x()*yc,	xz = x()*zc;
		FT yy = y()*yc,	yz = y()*zc,	zz = z()*zc;
		mat.m[0][0] = FT(1.0 - (yy + zz));
		mat.m[0][1] = FT(xy - sz);
		mat.m[0][2] = FT(xz + sy);
		mat.m[1][0] = FT(xy + sz);
		mat.m[1][1] = FT(1.0 - (xx + zz));
		mat.m[1][2] = FT(yz - sx);
		mat.m[2][0] = FT(xz - sy);
		mat.m[2][1] = FT(yz + sx);
		mat.m[2][2] = FT(1.0 - (xx + yy));
		mat.m[0][3] = mat.m[1][3] = mat.m[2][3] = mat.m[3][0] = mat.m[3][1] = mat.m[3][2] = 0.0f;
		mat.m[3][3] = 1.0f;
		return mat;
	}

	// Per coordinate (explicit inline functions)
	void s(const FT s)	{ m_s = s; }
	void v(const Vector3 v)	{ m_v = v; }
	void x(const FT x)	{ m_v.setx(x); }
	void y(const FT y)	{ m_v.sety(y); }
	void z(const FT z)	{ m_v.setz(z); } 

	// Data access (explicit inline functions)
	FT	s() const { return m_s; }
	Vector3	v() const { return m_v; }
	FT	x() const { return m_v.x; }
	FT	y() const { return m_v.y; }
	FT	z() const { return m_v.z; }

	FT& operator[](int i)
    {
		if (i==0) return (FT&)m_s;
		else return (FT&)m_v[i-1];
	}
	const FT& operator[](int i) const
    {
		if (i==0) return (FT&)m_s;
		else return (FT&)m_v[i-1];
	}
	
	// Operators
	CQuaternion& operator+=(const CQuaternion& rQuad)
	{
		m_s += rQuad.s();
		m_v += rQuad.v();
		return *this;
	}
	CQuaternion& operator+=(const CQuaternion* pQuad)
	{
		m_s += pQuad->s();
		m_v += pQuad->m_v;
		return *this;
	}
	CQuaternion& operator-=(const CQuaternion& rQuad)
	{
		m_s -= rQuad.s();
		m_v -= rQuad.v();
		return *this;
	}
	CQuaternion& operator-=(const CQuaternion* pQuad)
	{
		m_s -= pQuad->s();
		m_v -= pQuad->v();
		return *this;
	}
	CQuaternion& operator*=(const FT d)
	{
		m_s *= d;
		m_v *= d;
		return *this;
	}
	CQuaternion& operator/=(const FT d)
	{
		return *this *= (1.0 / d);
	}

	// Nondestructive unary negation - returns a new vector
	CQuaternion  operator -() const
	{
		return CQuaternion(-m_s, -m_v);
	}

	// Binary operators
	friend CQuaternion operator+(const CQuaternion& u, const CQuaternion& v)
	{
		return CQuaternion(u.m_s+v.m_s, u.m_v+v.m_v);
	}
	friend CQuaternion operator-(const CQuaternion& u, const CQuaternion& v)
	{
		return CQuaternion(u.m_s-v.m_s, u.m_v-v.m_v);
	}
	friend CQuaternion operator*(const CQuaternion& u, const CQuaternion& v)
	{
		CQuaternion w;
		Vector3 a  = u.m_v;
		Vector3 b  = v.m_v;
		FT  ws = u.m_s * v.m_s - a.dot(b);
		Vector3 wv = b*u.m_s + a*v.m_s + a.cross(b);
		w.Set(ws,wv);
		return w;
	}
	friend CQuaternion operator*(const FT s, const CQuaternion& u)
	{
		return CQuaternion(u.m_s * s, u.m_v * s);
	}
	friend CQuaternion operator*(const CQuaternion& u, const FT s)
    {
		return s * u;
	}
	friend CQuaternion operator/(const CQuaternion& u, const FT s)
    {
		return (1.f/s) * u;
	}
	friend int operator==(const CQuaternion& q1, const CQuaternion& q2)
	{
		return (q1.m_s == q2.m_s && q1.m_v == q2.m_v);
	}
	friend int operator!=(const CQuaternion& q1, const CQuaternion& q2)
    {
		return !(q1 == q2);
	}

	int Equals(const CQuaternion& q, const FT tolerence) const
	{
		CQuaternion diff = *this - q;
		return diff.LengthSquared() <= tolerence;
	}


	// Misc
	FT Normalize()
	{
		FT len = Length();
		if (len != 0.0f) {
			(*this) *= (1.0/len);
		} else {
			Set(0.f,0.f,0.f,0.f);
		}
		return len;
	}
	FT Length() const
	{
		return sqrt((FT)m_s*(FT)m_s + 
					(FT)m_v.x*(FT)m_v.x + 
					(FT)m_v.y*(FT)m_v.y + 
					(FT)m_v.z*(FT)m_v.z);
	}
	FT LengthSquared() const
	{
		return ((FT)m_s*(FT)m_s + 
				(FT)m_v.x*(FT)m_v.x + 
				(FT)m_v.y*(FT)m_v.y + 
				(FT)m_v.z*(FT)m_v.z);
	}
	void Negate()
	{
		m_s = -m_s;
		m_v = -m_v;
	}
	CQuaternion Conjugate()
	{
		return CQuaternion(m_s, -m_v[0], -m_v[1], -m_v[2]);
	}
};

#endif // _QUATERNION_
